import numpy as np
from utils2 import *
import pickle
import os

round1_feature_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/supplementSampleByHumanNpyFiles/320pairWithRound1Supplement_moreDataAug_grayOpticalFlow/round1.npy'
round2_feature_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/supplementSampleByHumanNpyFiles/320pairWithRound1Supplement_moreDataAug_grayOpticalFlow/round2.npy'
round3_feature_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/supplementSampleByHumanNpyFiles/320pairWithRound1Supplement_moreDataAug_grayOpticalFlow/round3.npy'

# 以下参数基本不会改的
round1_position_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round1/round1_sync_position.npy' # 经纬度
round2_position_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/round2_sync_position.npy' # 经纬度
round3_position_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round3/round3_sync_position.npy' # 经纬度
anchor_training_index_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorImgs_method2_exp2/round2_method2_exp2_training_index.txt'
anchor_referenceLabel_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorReferenceLabel_method2_exp2.txt'
pkuBirdViewImg = 'D:\\Research\\2020ContrastiveLearningForSceneLabel\\Data\\pkuBirdView.png'
index_map = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/IndexMap.pickle'
allImage_referenceLabel_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/allImageReferenceLabel.txt'

index_map = open(index_map,'rb')
index_map = pickle.load(index_map)
index_map = index_map[1]

# 锚点-正样本，锚点-负样本列表
anchor_pos_list, anchor_neg_list = getAnchorPosNegIdx4(anchor_training_index_path)
anchor_idx = [anchor_pos_list[i][0] for i in range(len(anchor_pos_list))]
training_idx = []
for i in anchor_pos_list:
    training_idx += i[1]

f = open(anchor_referenceLabel_path,'r')
lines = f.readlines()
f.close()
rf_label_anchor = [int(i[0]) for i in lines]
rf_label_training = [rf_label_anchor[i] for i in range(len(rf_label_anchor)) for j in range(16)]

rf_label_anchor = np.array(rf_label_anchor)
rf_label_training = np.array(rf_label_training)

f = open(allImage_referenceLabel_path,'r') # 为了标注方便（在键盘上敲1,2,3比敲0,1,2要方便很多），定义1直路，2弯道，3 Alerting
lines = f.readlines()
rf_label_allFrame = []
for line in lines:
    label = int(line.split()[1])
    rf_label_allFrame.append(label-1) # 分类时的编号是0,1,2，故这里减1
f.close()

rotate = 0 # 角度制
shiftX = 625
shiftY = 620
dx = 0.777
dy = 0.777 # 以上参数都是手调的
alpha = 0.5
GNSS_round2 = np.load(round2_position_path)
GNSS_round2[:,0] = -GNSS_round2[:,0]
GNSS_round2[:,1] *= dx
GNSS_round2[:,0] *= dy
GNSS_round2[:,1] += shiftX
GNSS_round2[:,0] += shiftY
GNSS_round1 = np.load(round1_position_path)
GNSS_round1[:,0] = -GNSS_round1[:,0]
GNSS_round1[:,1] *= dx
GNSS_round1[:,0] *= dy
GNSS_round1[:,1] += shiftX
GNSS_round1[:,0] += shiftY
GNSS_round3 = np.load(round3_position_path)
GNSS_round3[:,0] = -GNSS_round3[:,0]
GNSS_round3[:,1] *= dx
GNSS_round3[:,0] *= dy
GNSS_round3[:,1] += shiftX
GNSS_round3[:,0] += shiftY

cmap = 'rainbow'

round1_feat = np.load(round1_feature_path)
round2_feat = np.load(round2_feature_path)
round3_feat = np.load(round3_feature_path)

round2_training_feat = round2_feat[training_idx]
pca = decomposition.PCA(n_components=2)
pca.fit(round2_training_feat)
round2_training_feat_pca = pca.transform(round2_training_feat)


# 计算round2每一帧的DS
round2_ds_path = round2_feature_path[0:-4] + '_DS.npy'
if not os.path.exists(round2_ds_path):
    round2_frame_num = np.shape(round2_feat)[0]
    round2_all_feat_sim = np.matmul(round2_feat, round2_feat.transpose(1,0))
    round2_ds = []
    bins = 100
    for i in range(round2_frame_num):
        cur_sim = round2_all_feat_sim[i]
        mask = np.ones(round2_frame_num, dtype=bool)
        mask[i] = False
        cur_sim = cur_sim[mask]
        cur_hist = np.histogram(cur_sim, bins=bins, weights = np.zeros_like(cur_sim) + 1 / len(cur_sim) ,range=(-1,1))[0]
        round2_ds.append(cur_hist)

    round2_ds = np.array(round2_ds)
    np.save(round2_ds_path, round2_ds)
else:
    round2_ds = np.load(round2_ds_path)

pass